{
  "nbformat": 4,
  "metadata": {
    "language_info": {
      "file_extension": ".py",
      "name": "python",
      "codemirror_mode": {
        "version": 3,
        "name": "ipython"
      },
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "mimetype": "text/x-python",
      "version": "3.5.2"
    },
    "kernelspec": {
      "language": "python",
      "display_name": "Python 3",
      "name": "python3"
    }
  },
  "cells": [
    {
      "metadata": {
        "collapsed": false
      },
      "source": [
        "%matplotlib inline"
      ],
      "execution_count": null,
      "outputs": [],
      "cell_type": "code"
    },
    {
      "metadata": {},
      "source": [
        "\n********************************************************************************\nAttitude Estimation with an IMU - Example\n********************************************************************************\nGoal of this script:\n\n- applying the UKF for estimating 3D attitude from an IMU.\n\n*We assume the reader is already familiar with the tutorial.*\n\nAttitude estimation with an Inertial Measurement Unit (IMU). The filter fuses\nmeasurements coming from gyros, accelerometers and magnetometers. The IMU does\nnot have any bias. We reproduce the simulation based on :cite:`kokUsing2017`.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Import\n==============================================================================\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {
        "collapsed": false
      },
      "source": [
        "from scipy.linalg import block_diag\nfrom ukfm import ATTITUDE as MODEL\nimport ukfm\nimport numpy as np\nimport matplotlib\nukfm.utils.set_matplotlib_config()"
      ],
      "execution_count": null,
      "outputs": [],
      "cell_type": "code"
    },
    {
      "metadata": {},
      "source": [
        "Model and Simulation\n==============================================================================\nThis script uses the :meth:`~ukfm.ATTITUDE` model that requires  the sequence\ntime and the IMU frequency.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# sequence time (s)\nT = 100\n# IMU frequency (Hz)\nimu_freq = 100\n# create the model\nmodel = MODEL(T, imu_freq)"
      ],
      "execution_count": null,
      "outputs": [],
      "cell_type": "code"
    },
    {
      "metadata": {},
      "source": [
        "The true trajectory is computed along with noisy inputs after we define the\nnoise standard deviation affecting the IMU, where the platform is 2 s\nstationary and then has constant angular velocity around gravity.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# IMU noise standard deviation (noise is isotropic)\nimu_std = np.array([5/180*np.pi,  # gyro (rad/s)\n                    0.4,          # accelerometer (m/s^2)\n                    0.2])         # magnetometer\n# simulate true trajectory and noisy inputs\nstates, omegas = model.simu_f(imu_std)"
      ],
      "execution_count": null,
      "outputs": [],
      "cell_type": "code"
    },
    {
      "metadata": {},
      "source": [
        "The state and the input contain the following variables:\n\n.. highlight:: python\n.. code-block:: python\n\n      states[n].Rot      # 3d orientation (matrix)\n      omegas[n].gyro     # robot angular velocities\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "We compute noisy measurements based on the true states.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {
        "collapsed": false
      },
      "source": [
        "ys = model.simu_h(states, imu_std)"
      ],
      "execution_count": null,
      "outputs": [],
      "cell_type": "code"
    },
    {
      "metadata": {},
      "source": [
        "A measurement ``ys[k]`` contains accelerometer and magnetometer measurements.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Filter Design and Initialization\n------------------------------------------------------------------------------\nWe embed the state in $SO(3)$ with left multiplication, such that:\n\n- the retraction $\\varphi(.,.)$ is the $SO(3)$ exponential\n  where the state multiplies the uncertainty on the left.\n\n- the inverse retraction $\\varphi^{-1}_.(.)$ is the $SO(3)$\n  logarithm.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# propagation noise covariance matrix\nQ = imu_std[0]**2*np.eye(3)\n# measurement noise covariance matrix\nR = block_diag(imu_std[1]**2*np.eye(3), imu_std[2]**2*np.eye(3))\n# initial uncertainty matrix\nP0 = np.zeros((3, 3))  # The state is perfectly initialized\n# sigma point parameters\nalpha = np.array([1e-3, 1e-3, 1e-3])"
      ],
      "execution_count": null,
      "outputs": [],
      "cell_type": "code"
    },
    {
      "metadata": {},
      "source": [
        "We initialize the filter with the true state.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {
        "collapsed": false
      },
      "source": [
        "state0 = model.STATE(Rot=states[0].Rot)\nukf = ukfm.UKF(state0=state0,\n               P0=P0,\n               f=model.f,\n               h=model.h,\n               Q=Q,\n               R=R,\n               phi=model.phi,\n               phi_inv=model.phi_inv,\n               alpha=alpha)\n# set variables for recording estimates along the full trajectory\nukf_states = [state0]\nukf_Ps = np.zeros((model.N, 3, 3))\nukf_Ps[0] = P0"
      ],
      "execution_count": null,
      "outputs": [],
      "cell_type": "code"
    },
    {
      "metadata": {},
      "source": [
        "Filtering\n==============================================================================\nThe UKF proceeds as a standard Kalman filter with a for loop.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {
        "collapsed": false
      },
      "source": [
        "for n in range(1, model.N):\n    # propagation\n    ukf.propagation(omegas[n-1], model.dt)\n    # update\n    ukf.update(ys[n])\n    # save estimates\n    ukf_states.append(ukf.state)\n    ukf_Ps[n] = ukf.P"
      ],
      "execution_count": null,
      "outputs": [],
      "cell_type": "code"
    },
    {
      "metadata": {},
      "source": [
        "Results\n------------------------------------------------------------------------------\nWe plot the orientation as function of time and the orientation error.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {
        "collapsed": false
      },
      "source": [
        "model.plot_results(ukf_states, ukf_Ps, states, omegas)"
      ],
      "execution_count": null,
      "outputs": [],
      "cell_type": "code"
    },
    {
      "metadata": {},
      "source": [
        "The trajectory starts by a small stationary step following by constantly\nturning around the gravity vector (only the yaw is increasing).\n\nWe have plotted the 95% ($3\\sigma$) confident interval and see the error\nis mainly below behind this interval: in this situation the filter covariance\noutput matches especially well the error behavior.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Conclusion\n==============================================================================\nThis script shows how well works the UKF on parallelizable manifolds for\nestimating the orientation of a platform from an IMU.\n\nYou can now:\n\n- address the UKF for the same problem with different noise parameters.\n\n- add outliers in acceleration or magnetometer measurements.\n\n- benchmark the UKF with different retractions and compare it to the\n  extended Kalman filter in the Benchmarks section.\n\n"
      ],
      "cell_type": "markdown"
    }
  ],
  "nbformat_minor": 0
}